import cv2 as cv
import numpy as np
from Find_Contour2 import findContours
from kalmanfilter import KalmanFilter

kf = KalmanFilter()
#类 摄像头
class Camera():
    #fps:摄像头帧率 cap:cv.VideoCapture打开的文件或摄像头
    def __init__(self,fps,cap):
        self.fps = fps
        self.cap = cap
    #摄像头参数设置    
    def Open(self):
        self.cap.set(cv.CAP_PROP_FPS, self.fps)
        return self.cap
    #显示
    def Show(self,name,frame):
        #name：窗口命名
        cv.namedWindow(name, cv.WINDOW_AUTOSIZE)
        cv.imshow(name, frame)
    #关闭相机
    def Quit(self):
        self.cap.release()
        cv.destroyAllWindows()
#类 目标物体
class Object(): 
    #检测目标物体，找到装甲板中心点
    def Dec(self,frame):
        #获得轮廓坐标
        Final_Box,i = findContours(frame,7,2,50)
        #当检测到至少一个目标时
        if i > 0:
            for X in Final_Box:
                Y = np.array(X)
                cv.drawContours(frame, [Y], 0, (0, 255, 0), 2)
        return Final_Box,i

    def Cal(self,frame,Final_Box):
        
        MID = []#储存装甲板中心点坐标
        for a in Final_Box:
            #print(a)
            midx = int((a[0][0] + a[2][0])/2)
            midy = int((a[0][1] + a[2][1])/2)
            cv.circle(frame, (midx, midy), 10, (0, 0, 255), 3)
            MID.append([midx,midy])
        return MID
        
        
        '''if i > 0:
            x, y, x2, y2 = light_bbox[0]
            cx = int((x + x2) / 2)
            cy = int((y + y2) / 2)
            cv.rectangle(frame, (x, y), (x2, y2), (0, 0, 255), 4)
            if i == 2:
                X, Y, X2, Y2 = light_bbox[1]
                cX = int((X + X2) / 2)
                cY = int((Y + Y2) / 2)
                cv.rectangle(frame, (X, Y), (X2, Y2), (0, 0, 255), 4)
                a = int((cx + cX) / 2)
                b = int((cy + cY) / 2)
                cv.circle(frame, (a, b), 10, (0, 0, 255), 4)'''

        #cv.circle(frame, (a, b), 10, (0, 0, 255), 4)
        '''
        #x, y, x2, y2 = light_bbox
        
        #两个矩形各自中心点
        cx = int((x + x2) / 2)
        cy = int((y + y2) / 2)
        cX = int((X + X2) / 2)
        cY = int((Y + Y2) / 2)
        #cx = int((x + x2) / 2)
        #cy = int((y + y2) / 2)

        #计算矩形长宽
        if x2 == x or y2 == y:
            width = 0.000001
            height = 0.000001
        else:
            width = (abs(x2-x)) * UP_W
            height = (abs(y2-y)) * UP_H
        
        #装甲板中心
        a=b=0
        a = int((cx + cX) / 2)
        b = int((cy + cY) / 2)
        
        #画图
        cv.rectangle(frame, (x, y), (x2, y2), (0, 0, 255), 4)
        cv.rectangle(frame, (X, Y), (X2, Y2), (0, 0, 255), 4)
        #cv.rectangle(frame, (x, y), (x2, y2), (0, 0, 255), 4)

        #cv.circle(frame, (a, b), 10, (0, 0, 255), 4)
        #cv.circle(frame, (cx, cy), 10, (0, 0, 255), 4)
        #cv.circle(frame, (cX, cY), 10, (0, 0, 255), 4)
        
        #cv.circle(frame, (cx, cy), 20, (0, 0, 255), 4)
        #return cx,cy,width,height'''
        
    
    def Pre(self,MID):
        #求出装甲板中心点坐标
        Midx = int((MID[0][0] + MID[1][0])/2)
        Midy = int((MID[0][1] + MID[1][1])/2)
        cv.circle(frame, (Midx, Midy), 7, (0, 0, 255), 3)
        #用卡尔曼滤波预测
        predicted = kf.predict(Midx,Midy)
        #画出预测点
        cv.circle(frame, (predicted[0], predicted[1]), 7, (255, 0, 0), 4)
        
#创建两个类变量
c1 = Camera(25,cv.VideoCapture("D:/All_About_Study/装甲板测试视频.avi"))
o1 = Object()

CAP = c1.Open()
fps = CAP.get(cv.CAP_PROP_FPS)

while True:
    ret, frame = CAP.read()
    if ret is False:
        break
    #目标检测
    Final_Box ,i = o1.Dec(frame)
    #检测到一个轮廓时才进行计算
    if i > 0: 
        MID = o1.Cal(frame,Final_Box)
        #检测到两个轮廓时，即存在中心点时才进行预测
        if i > 1:
            o1.Pre(MID)
    
    c1.Show('video',frame)
    cv.waitKey(int(fps))
    if cv.waitKey(1) == 27:
        break
c1.Quit()       